/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright, Nick Johnson, Josep Bosch
*
*********************************************************************/

#ifndef _PILOTNODE_H_
#define _PILOTNODE_H_

// ================================================================== includes

// ModuleCore include should be above ROS and OceanSHELL includes.
#include <osl_core/ModuleCore.h>
#include <osl_core/Chrono.h>
#include <osl_core/TimerMillis.h>

#include <matchmaker/client.h>

#include <auv_msgs/NavSts.h>
#include <auv_msgs/WorldWaypointReq.h>
#include <auv_msgs/BodyWaypointReq.h>
#include <auv_msgs/StayWaypointReq.h>
#include <auv_msgs/BodyVelocityReq.h>
#include <auv_msgs/ThrusterForce.h>
#include <auv_msgs/WaypointSts.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/TwistStamped.h>

#include <auv_pilot/pilot.h>

// ========================================================== external defines

typedef enum
{
	THRUSTER_MODE_INVALID = 0,
	THRUSTER_MODE_HOVER = 1,
	THRUSTER_MODE_TRANSIT = 2

} thruster_mode_t;
// ===================================================================== class
class Autopilot : public osl_core::ModuleCore
{
private:
	bool init(int argc, char * argv []);
	void doWork();
	void cleanup();

	void processROSNavSts(const auv_msgs::NavStsConstPtr & msg);
	void processROSWorldReq(const auv_msgs::WorldWaypointReqConstPtr & msg);
	void processROSBodyReq(const auv_msgs::BodyWaypointReqConstPtr & msg);
	void processROSStayReq(const auv_msgs::StayWaypointReqConstPtr & msg);
	void processROSManualReq(const geometry_msgs::WrenchStampedConstPtr & msg);
	void processROSOverrideForceReq(const geometry_msgs::WrenchStampedConstPtr & msg);
	void processROSVelocityReq(const auv_msgs::BodyVelocityReqConstPtr & msg);

	void findServices();

	void navServiceStatusCallback(const matchmaker::ServiceDetails& service);

	void updateServiceStatus();

	void sendNavGoalSts();

	void sendForceMsg();
	void sendZeroForceMsg();

	void chooseThrusterModeFromNav(const auv_msgs::NavStsConstPtr & msg);

	void chooseThrusterModeFromPoseError(const auv_pilot::BodyFrameNumeric<double>& poseError);

	void sendInPositionMsg();	
	
	double calcBearingFromNEOffset(double north, double east);
	
	ros::Subscriber m_rosNavStsSub;
	matchmaker::ServiceMonitor m_navServiceMon;

	ros::Subscriber m_rosWorldReqSub;
	matchmaker::ServiceAdvertiser m_worldReqServiceAd;

	ros::Subscriber m_rosBodyReqSub;
	matchmaker::ServiceAdvertiser m_bodyReqServiceAd;

	ros::Subscriber m_rosStayReqSub;
	matchmaker::ServiceAdvertiser m_stayReqServiceAd;

	ros::Subscriber m_rosManualReqSub;
	matchmaker::ServiceAdvertiser m_manualReqServiceAd;

	ros::Subscriber m_rosOverrideForceReqSub;

	ros::Subscriber m_rosVelocityReqSub;
	matchmaker::ServiceAdvertiser m_velocityReqServiceAd;

	ros::Publisher m_rosWaypointStsPub;
	matchmaker::ServiceAdvertiser m_waypointStsServiceAd;

	ros::Publisher m_rosAxisForcePub;

	ros::Publisher m_rosThrustReqPub;

	ros::Timer m_findServicesTimer;

	auv_pilot::Pilot m_pilot;
	
	osl_core::TimerMillis m_updateTimer;
	osl_core::Chrono *m_updateCrono;
	
	int m_updatePeriod;
	
	bool m_receivingNav;
	osl_core::TimerMillis m_navIdleTimer;
	int m_navIdleTimeout;
	
	bool m_autopilotActive;

	bool m_manualMode;
	osl_core::TimerMillis m_manualModeTimer;
	int m_manualModeTimeout;

	bool m_overrideForceMode;
	auv_pilot::BodyFrameNumeric<double> m_overrideForces;
	
	bool m_velocityMode;
	osl_core::TimerMillis m_velocityModeTimer;
	int m_velocityModeTimeout;
	
	osl_core::TimerMillis m_diagTimer;
	int m_diagPeriod;

	std::string m_autoposINIFilename;
	
	uint32 m_lastWaypointType;

	float m_transitOnXVel;
	float m_transitOffXVel;

	float m_transitOnXError;
	float m_transitOffXError;
	thruster_mode_t	m_thrusterMode;
};

#endif /* _PILOTNODE_H_ */
